﻿import time

from MPU6050 import MPU6050



def read_rpy_data(mpu,packet_size):

        FIFO_buffer = [0]*64
        try:
            FIFO_count = mpu.get_FIFO_count()
            mpu_int_status = mpu.get_int_status()
            # print(FIFO_count)
            # print(mpu_int_status)
        except:
            return None
        # If overflow is detected by status or fifo count we want to reset
        if (FIFO_count == 1024) or (mpu_int_status & 0x10):
            mpu.reset_FIFO()
            print('overflow!')
        # Check if fifo data is ready
        elif (mpu_int_status & 0x02):
            # Wait until packet_size number of bytes are ready for reading, default
            # is 42 bytes
            while FIFO_count < packet_size:
                FIFO_count = mpu.get_FIFO_count()

            FIFO_buffer = mpu.get_FIFO_bytes(packet_size)
            accel = mpu.DMP_get_acceleration_int16(FIFO_buffer)
            quat = mpu.DMP_get_quaternion_int16(FIFO_buffer)
            grav = mpu.DMP_get_gravity(quat)
            roll_pitch_yaw = mpu.DMP_get_euler_roll_pitch_yaw(quat, grav)
            str_show = "roll: %.2f  pitch: %.2f  yaw: %.2f        "%(roll_pitch_yaw.x,roll_pitch_yaw.y,roll_pitch_yaw.z)
            print("\r %s"%(str_show),end='')
            # return roll_pitch_yaw









if __name__ == "__main__":
    i2c_bus = 0
    device_address = 0x68

    x_accel_offset = int(-1581.625)
    y_accel_offset = int(1510.5)
    z_accel_offset = int(-778.875)
    x_gyro_offset = int(16.75)
    y_gyro_offset = int(-14.75)
    z_gyro_offset = int(-21.0)

    mpu = MPU6050(i2c_bus, device_address, x_accel_offset, y_accel_offset,z_accel_offset, x_gyro_offset, y_gyro_offset, z_gyro_offset,False)

    mpu.dmp_initialize()
    mpu.set_DMP_enabled(True)
    mpu_int_status = mpu.get_int_status()

    packet_size = mpu.DMP_get_FIFO_packet_size()

    FIFO_count = mpu.get_FIFO_count()
    print(packet_size)
    print(FIFO_count)


    try:
        while True:

            read_rpy_data(mpu,packet_size)
    
            time.sleep(0.1)

    except KeyboardInterrupt:
        print('\n Ctrl + C QUIT')
